#include "mpu6050.h"

#include <math.h>

#include "hal_mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"

#undef Handler
#define Handler Mpu_HandleTypeDef*

#undef handler
#define handler (**(Mpu_HandleTypeDef**)&handle)

// q30格式,long转float时的除数.
#define q30 1073741824.0f

Mpu_HandleTypeDef hmpu;

uint8_t Mpu_Init(void) {
  if (mpu_dmp_init()) return 1;
  return 0;
}

void Mpu_Update(Mpu_HandleTypeDef* handle) {
  unsigned long sensor_timestamp;
  short gyro[3], accel[3], sensors;
  unsigned char more;
  dmp_read_fifo(gyro, accel, handler.quat, &sensor_timestamp, &sensors, &more);
  if (sensors & INV_WXYZ_QUAT) {
    handler.fq[0] = handler.quat[0] / q30;  // q30格式转换为浮点数
    handler.fq[1] = handler.quat[1] / q30;
    handler.fq[2] = handler.quat[2] / q30;
    handler.fq[3] = handler.quat[3] / q30;
  }
}

void Mpu_GetYaw(Mpu_HandleTypeDef* handle, float* yaw) {
  *yaw =
      atan2(2 * (handler.fq[1] * handler.fq[2] + handler.fq[0] * handler.fq[3]),
            handler.fq[0] * handler.fq[0] + handler.fq[1] * handler.fq[1] -
                handler.fq[2] * handler.fq[2] - handler.fq[3] * handler.fq[3]) *
      57.3;
}

void Mpu_GetVZ(Mpu_HandleTypeDef* handle, float* vz) {
  *vz = 1 - 2 * (handler.fq[1] * handler.fq[1] + handler.fq[2] * handler.fq[2]);
}
